/*
 * @Author: wangyingjie 2778809626@qq.com
 * @Date: 2025-07-03 15:35:52
 * @LastEditors: wangyingjie 2778809626@qq.com
 * @LastEditTime: 2025-07-08 11:32:07
 * @FilePath: /two_wheeled_car/src/bot_gazebo/src/MoveCylinderPlugin.cpp
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <ignition/math/Pose3.hh>

namespace gazebo
{
// 定义一个插件类，用于使圆柱体模型沿圆形路径移动
class MoveCylinderPlugin : public ModelPlugin
{
public:
    // Load 函数，在插件加载到模型时调用
    void Load(physics::ModelPtr model, sdf::ElementPtr /*sdf*/) override
    {
        // 存储模型指针
        this->model = model;
        // 禁用模型名为 "link" 的链接的重力
        this->model->GetLink("link")->SetGravityMode(false);
        
        // 初始化参数
        this->radius = 3;  // 圆形路径半径
        this->speed = 0.001;   // 移动速度（弧度/秒）

        // 连接到世界更新事件（在每次仿真迭代时调用）
        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
            std::bind(&MoveCylinderPlugin::OnUpdate, this));
    }

    // // 每次仿真迭代时调用的更新函数
    // void OnUpdate()
    // {
    //     // 根据当前时间计算新的位置
    //     double currentTime = this->model->GetWorld()->SimTime().Double();
    //     double x = this->radius * cos(this->speed * currentTime);
    //     double y = this->radius * sin(this->speed * currentTime);
    //     // 设置圆柱体的新姿态
    //     ignition::math::Pose3d newPose(x, y, 0.5, 0, 0, 0);  // z = 0.5 以保持其在地面上方
    //     this->model->SetWorldPose(newPose);
    // }

    void OnUpdate()
    {
        double currentTime = this->model->GetWorld()->SimTime().Double();
        double baseX = this->radius * cos(this->speed * currentTime);
        double baseY = this->radius * sin(this->speed * currentTime);
        // 添加小幅度随机扰动
        double noiseX = (rand() % 100 - 50) * 0.01; // 范围 [-0.5, 0.5]
        double noiseY = (rand() % 100 - 50) * 0.01;
        ignition::math::Pose3d newPose(baseX + noiseX, baseY + noiseY, 0.5, 0, 0, 0);
        this->model->SetWorldPose(newPose);
    }

private:
    physics::ModelPtr model;                 // 指向模型的指针
    event::ConnectionPtr updateConnection;   // 指向更新事件连接的指针
    double radius;                           // 圆周运动的半径
    double speed;                            // 移动速度
};

// 在 Gazebo 中注册此插件
GZ_REGISTER_MODEL_PLUGIN(MoveCylinderPlugin)
}
